#include <string.h>
#include "lib_pid.h"

void PID_init(pid_float *pid)
{
    pid->SetAngle = 850;
    pid->ActualAngle = 0.0;
    pid->err = 0.0;
    pid->err_last = 0.0;
    pid->delay_ms = 0.0;
    pid->integral = 0.0;
    pid->Kp = 1.3;
    pid->Ki = 0.12;
    pid->Kd = 0.16;
}
// void PID_realize(pid_float *pid){
//     pid->err=pid->SetAngle-pid->ActualAngle;
//     pid->integral+=pid->err;
//     pid->pwm=pid->Kp*pid->err+pid->Ki*pid->integral+pid->Kd*(pid->err-pid->err_last);
//     if (pid->pwm>0&&pid->pwm<30)
//     {
//         pid->pwm+=30;
// //        pid->err=0;
//     }
//     else if (pid->pwm<0&&pid->pwm>-30)
//     {
//         pid->pwm-=30;
// //        pid->err=0;
//     }

//     pid->err_last=pid->err;
// }
void PID_realize(pid_float *pid)
{
    float index = 0;
    pid->err = pid->SetAngle - pid->ActualAngle;
    if (abs(pid->err) < 5) //变积分过程
    {
        pid->err = 0;
        index = 0.0;
        pid->integral = 0;
        pid->err_last = 0;
    }
    if (abs(pid->err) > 50) //变积分过程
    {
        index = 0.0;
    }
    else if (abs(pid->err) < 50)
    {
        index = 1.0;
        pid->integral += pid->err;
    }
    else
    {
        index = (50 - abs(pid->err)) / 5;
        pid->integral += pid->err;
    }
    pid->delay_ms = pid->Kp * pid->err + index * pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last);
    pid->err_last = pid->err;
}